import time

import rtde_receive
import rtde_control
from compensation import GravityCompensation
from Hardware.KunWei_ftsensor import ftsensor
import sys

rtde_r = rtde_receive.RTDEReceiveInterface('192.168.68.250')
rtde_c = rtde_control.RTDEControlInterface('192.168.68.250')
ft = ftsensor(False)
compensation = GravityCompensation()
rtde_c.teachMode()

try:
    while True:
        # force = rtde_r.getFtRawWrench()
        force = ft.GetForce()
        q = rtde_r.getActualQ()
        rotation_vector = rtde_c.getForwardKinematics(q, [0, 0, 0, 0, 0, 0])
        # compensated_force = compensation.GetForceTorque(force, rotation_vector[3:6])
        compensated_force = compensation.GetForceTorque_On_Base(force, rotation_vector[3:6])
        sys.stdout.write("\r"+"raw_force:"+str(force)+"compensated_force:"+str(compensated_force))
        sys.stdout.flush()

except KeyboardInterrupt:
    ft.DisConnect()